#include "mpu6050.h"


float gyro_offset[3] = {0.05, -0.01, 0.02};//陀螺仪偏移量,根据实测填写,调用void print_gyro_kf()测量
Adafruit_MPU6050 mpu;
Matrix<3,1> x_1={0,0,10};
Matrix<3,3> P_1={1,0,0,0,1,0,0,0,1};
Matrix<3,3> Q_1={0.001,0,0,0,0.001,0,0,0,0.001};
Matrix<3,3> R_1={0.003,0,0,0,0.003,0,0,0,0.003};
Matrix<3,3> F_1={1,0,0,0,1,0,0,0,1};
Matrix<3,3> H_1={1,0,0,0,1,0,0,0,1};
Matrix<3,3> B_1={0,0,0,0,0,0,0,0,0};
KalmanFilter3D kalman_acc(x_1,P_1,Q_1,R_1,F_1,H_1,B_1);
Matrix<3,1> x_2={0,0,0};
Matrix<3,3> P_2={1,0,0,0,1,0,0,0,1};
Matrix<3,3> Q_2={0.001,0,0,0,0.001,0,0,0,0.001};
Matrix<3,3> R_2={0.003,0,0,0,0.003,0,0,0,0.003};
Matrix<3,3> F_2={1,0,0,0,1,0,0,0,1};
Matrix<3,3> H_2={1,0,0,0,1,0,0,0,1};
Matrix<3,3> B_2={0,0,0,0,0,0,0,0,0};
KalmanFilter3D kalman_gyro(x_2,P_2,Q_2,R_2,F_2,H_2,B_2);
Matrix<3,1> x_3={0,0,0};
Matrix<3,3> P_3={1,0,0,0,1,0,0,0,1};
Matrix<3,3> Q_3={0.001,0,0,0,0.001,0,0,0,0.001};
Matrix<3,3> R_3={0.003,0,0,0,0.003,0,0,0,0.003};
Matrix<3,3> F_3={1,0,0,0,1,0,0,0,1};
Matrix<3,3> H_3={1,0,0,0,1,0,0,0,1};
Matrix<3,3> B_3={0,0,0,0,0,0,0,0,0};
KalmanFilter3D kalman_angle(x_3,P_3,Q_3,R_3,F_3,H_3,B_3);
void mpu_init()
{
    Wire.begin(IIC_SDA, IIC_SCL,400000);
    if (!mpu.begin())
    {
        Serial.println("Failed to find MPU6050 chip");
        while (1)
        {
            delay(10);
        }
    }
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    Serial.print("Accelerometer range set to: ");
    switch (mpu.getAccelerometerRange())
    {
    case MPU6050_RANGE_2_G:
        Serial.println("+-2G");
        break;
    case MPU6050_RANGE_4_G:
        Serial.println("+-4G");
        break;
    case MPU6050_RANGE_8_G:
        Serial.println("+-8G");
        break;
    case MPU6050_RANGE_16_G:
        Serial.println("+-16G");
        break;
    }
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
    Serial.print("Gyro range set to: ");
    switch (mpu.getGyroRange())
    {
    case MPU6050_RANGE_250_DEG:
        Serial.println("+- 250 deg/s");
        break;
    case MPU6050_RANGE_500_DEG:
        Serial.println("+- 500 deg/s");
        break;
    case MPU6050_RANGE_1000_DEG:
        Serial.println("+- 1000 deg/s");
        break;
    case MPU6050_RANGE_2000_DEG:
        Serial.println("+- 2000 deg/s");
        break;
    }
    mpu.setFilterBandwidth(MPU6050_BAND_44_HZ);
    Serial.print("Filter bandwidth set to: ");
    switch (mpu.getFilterBandwidth())
    {
    case MPU6050_BAND_260_HZ:
        Serial.println("260 Hz");
        break;
    case MPU6050_BAND_184_HZ:
        Serial.println("184 Hz");
        break;
    case MPU6050_BAND_94_HZ:
        Serial.println("94 Hz");
        break;
    case MPU6050_BAND_44_HZ:
        Serial.println("44 Hz");
        break;
    case MPU6050_BAND_21_HZ:
        Serial.println("21 Hz");
        break;
    case MPU6050_BAND_10_HZ:
        Serial.println("10 Hz");
        break;
    case MPU6050_BAND_5_HZ:
        Serial.println("5 Hz");
        break;
    }
}

void mpu_test(int mode ) {

  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
if (mode == 0) {
  /* Print out the values */
  Serial.print("Acceleration X: ");
  Serial.print(a.acceleration.x);
  Serial.print(", Y: ");
  Serial.print(a.acceleration.y);
  Serial.print(", Z: ");
  Serial.print(a.acceleration.z);
  Serial.println(" m/s^2");
}
else if (mode==1) {
  Serial.print("Rotation X: ");
  Serial.print(g.gyro.x);
  Serial.print(", Y: ");
  Serial.print(g.gyro.y);
  Serial.print(", Z: ");
  Serial.print(g.gyro.z);
  Serial.println(" rad/s");
}
else if(mode==3){
  Serial.print("Temperature: ");
  Serial.print(temp.temperature);
  Serial.println(" degC");

  Serial.println("");
}
//   delay(500);
}

void print_acc_kf(){
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    Matrix<3,1> z={a.acceleration.x,a.acceleration.y,a.acceleration.z};
    kalman_acc.predict();
    kalman_acc.update(z);
    Serial.print("Acceleration X: ");
    Serial<<a.acceleration.x<<", Y: "<<a.acceleration.y<<", Z: "<<a.acceleration.z<<" m/s^2  ";
    Serial<<kalman_acc.x(0)<<", Y: "<<kalman_acc.x(1)<<", Z: "<<kalman_acc.x(2)<<" m/s^2\n";
    
}

void print_gyro_kf(){
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    Matrix<3,1> z={g.gyro.x-gyro_offset[0],g.gyro.y-gyro_offset[1],g.gyro.z-gyro_offset[2]};
    kalman_gyro.predict();
    kalman_gyro.update(z);
    Serial.print("Rotation X: ");
    Serial<<g.gyro.x<<", Y: "<<g.gyro.y<<", Z: "<<g.gyro.z<<" rad/s  ";
    Serial<<kalman_gyro.x(0)<<", Y: "<<kalman_gyro.x(1)<<", Z: "<<kalman_gyro.x(2)<<" rad/s";
    Serial<<kalman_gyro.K(0,0)<<", Y: "<<kalman_gyro.K(1,1)<<", Z: "<<kalman_gyro.K(2,2)<<"\n";
    // Serial<<kalman_gyro.K<<"\n";
}

void get_acc_gyro_anlge_kf(float &gx,float &gy ,float &gz,float &ax,float &ay,float &az)
{
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    Matrix<3,1> z={a.acceleration.x,a.acceleration.y,a.acceleration.z};
    kalman_acc.predict();
    kalman_acc.update(z);
    Matrix<3,1> z1={g.gyro.x-gyro_offset[0],g.gyro.y-gyro_offset[1],g.gyro.z-gyro_offset[2]};
    kalman_gyro.predict();
    kalman_gyro.update(z1);
    ax=kalman_acc.x(0);
    ay=kalman_acc.x(1);
    az=kalman_acc.x(2);
    gx=kalman_gyro.x(0);
    gy=kalman_gyro.x(1);
    gz=kalman_gyro.x(2);
}
void print_acc_raw(){
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
}
void print_gyro_raw(){
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
}